#include <Dayond/Dayond.h>
#include "bsp/bsp_can2.h"

const static uint8_t base_cmd[] = {0x00, 0x8a, 0x06, 0x00, 0x00, 0x00, 0x00, 0x01};

//ID: 00  DATA: 00 8A 06 01 00 00 00 01           //正转
//ID: 00  DATA: 00 8A 06 FE FF 00 00 01           //反转
//ID: 00  DATA: 00 8A 06 00 00 00 00 01           //停止 / 使能
//ID: 00  DATA: 00 8A 06 00 00 00 00 00           //停止 / 失能

void dy_init(void)
{
    dy_all_disable();
}

static void base_cmd_func(int vel, int cmd)
{
    struct rt_can_msg msg;
    int16_t v = vel * 8192 / 3000;

    msg.ide = RT_CAN_STDID;        // 标准格式
    msg.rtr = RT_CAN_DTR;          // 数据帧
    msg.len = 8;                   // 数据长度
    msg.id = 3;

    memcpy(msg.data, base_cmd, sizeof(msg.data));

    msg.data[3] = (v >> 8) & 0xff;
    msg.data[4] = (v >> 0) & 0xff;

    msg.data[7] = cmd;

    bsp_can2_tx_transmit(&msg);
}

void dy_all_run(int vel)
{
    base_cmd_func(vel, 1);
}

void dy_all_enable(void)
{
    base_cmd_func(0, 1);
}

void dy_all_disable(void)
{
    base_cmd_func(0, 0);
}
